#include <stdio.h>
#include <math.h>
#include <time.h>
#include <linux/i2c-dev.h>
#include <fcntl.h>

#include <stdlib.h>
#include <unistd.h>

#include "tick.h"
#include <wiringPi.h>
#include "BrickPi.h"

#define SPEED_LIMIT	300

int main(int argc, char *argv[])
{
	int result;
	int xyw[3];
	int motor[4];	// 0:LeftFront 1:LeftBack 2:RightBack 3:RightFront
	int speed[4];
	int i, j;

	// initialize.
	for (i = 0; i < 3; i++)	xyw[i] = 0;
	for (i = 0; i < 4; i++)	speed[i] = 0;

	// get args.
	j = argc;
	if (j > 4)	j = 4;
	for (i = 1; i < j; i++)
	{
		xyw[i-1] = atoi(argv[i]);
		if (xyw[i-1] < -100)	xyw[i-1] = -100;
		if (xyw[i-1] > 100)	xyw[i-1] = 100;
	}

	// caliculate.
	speed[0] = (int)(6.0*((double)xyw[0]/3 + (double)xyw[1]/3 + (double)xyw[2]/3));
	speed[1] = (int)(-6.0*(-(double)xyw[0]/3 + (double)xyw[1]/3 + (double)xyw[2]/3));
	speed[2] = (int)(-6.0*((double)xyw[0]/3 + (double)xyw[1]/3 - (double)xyw[2]/3));
	speed[3] = (int)(6.0*(-(double)xyw[0]/3 + (double)xyw[1]/3 - (double)xyw[2]/3));
	for (i = 0; i < 4; i++)
	{
		if (speed[i] > SPEED_LIMIT)	speed[i] = SPEED_LIMIT;
		if (speed[i] < -1 * SPEED_LIMIT)	speed[i] = -1 * SPEED_LIMIT;
	}

	printf("LF= %d , RF= %d , LB = %d , RB = %d\n", speed[0], speed[3], speed[1], speed[2]);

	// -- initialize BrickPi.
	// Clear Timer.
	ClearTick();

	// Initialize. (Open Port)
	result = BrickPiSetup();
	if (result)	return 0;

	// Set Address.
	BrickPi.Address[0] = 1;
	BrickPi.Address[1] = 2;

	// select the ports to be used by the motors.
	motor[0] = PORT_A;	// 0:LeftFront
	motor[1] = PORT_B;	// 1:LeftBack
	motor[2] = PORT_C;	// 2:RightBack
	motor[3] = PORT_D;	// 3:RightFront
	// enable motors.
	for (i = 0; i < 4; i++)
		BrickPi.MotorEnable[motor[i]] = 1;

	// setup the properties of sensors for the BrickPi.
	result = BrickPiSetupSensors();
	// set timeout.
	BrickPi.Timeout = 3000;
	BrickPiSetTimeout();

	if (!result)
	{
		for (i = 0; i < 4; i++)
		{
			BrickPi.MotorSpeed[motor[i]] = speed[i];
		}

		BrickPiUpdateValues();
	}

	return result;
}